#include "HectorMappingRos.h"
#include "shearcherMain.h"
#include "lidar_main.h"
#include "ReceivePartAn.h"
#include "socket.h"
#include <ctime>
#include <vector>
#include <math.h>
#include <unistd.h> 

#include "CSerialConnection.h"

using namespace everest::hwdrivers;

#define TARGETIP "192.168.2.179"

CSerialConnection *mySerial;

int threadNum[10];
pthread_t lidarThreadHandle;
pthread_t socketThreadHandle;
pthread_t searchThreadHandle;
pthread_t shareMemThreadHandle;

Bbox personBox;
uint8_t TxPack_Buffer[50];

union main
{
    int8_t data[sizeof(float)*3+sizeof(int)];
    struct 
    {
        int id;
        float X,Y,W;
    }pose;
}RobotInfo;

uint8_t carStopCmd()
{
    int8_t CK_A=0,CK_B=0;
    uint8_t _cnt=0;
    TxPack_Buffer[_cnt++]=0xFE;
    TxPack_Buffer[_cnt++]=0xFF;
    TxPack_Buffer[_cnt++]=0xC1;
    TxPack_Buffer[_cnt++]=0x01;
    TxPack_Buffer[_cnt++]=0;
    for(int i=0;i<_cnt;i++)
    {
        CK_A+=TxPack_Buffer[i];
        CK_B+=CK_A;
    }
    TxPack_Buffer[_cnt++]=CK_A;
    TxPack_Buffer[_cnt++]=CK_B;
    mySerial->write((char*)TxPack_Buffer,_cnt);
    return _cnt;
}

uint8_t Tx_APIC(Eigen::Vector2d target)
{
    int8_t CK_A=0,CK_B=0;
    uint8_t _cnt=0;
    float x=(float)target(0);
    float y=(float)target(1);
    {
        TxPack_Buffer[_cnt++]=0xFE;
        TxPack_Buffer[_cnt++]=0xFF;
        TxPack_Buffer[_cnt++]=0xC2;
        TxPack_Buffer[_cnt++]=0x02;
        TxPack_Buffer[_cnt++]=((uint8_t*)&x)[3];
        TxPack_Buffer[_cnt++]=((uint8_t*)&x)[2];
        TxPack_Buffer[_cnt++]=((uint8_t*)&x)[1];
        TxPack_Buffer[_cnt++]=((uint8_t*)&x)[0];

        TxPack_Buffer[_cnt++]=((uint8_t*)&y)[3];
        TxPack_Buffer[_cnt++]=((uint8_t*)&y)[2];
        TxPack_Buffer[_cnt++]=((uint8_t*)&y)[1];
        TxPack_Buffer[_cnt++]=((uint8_t*)&y)[0];

        for(int i=0;i<_cnt;i++)
        {
            CK_A+=TxPack_Buffer[i];
            CK_B+=CK_A;
        }
        TxPack_Buffer[_cnt++]=CK_A;
        TxPack_Buffer[_cnt++]=CK_B;
    }
    mySerial->write((char*)TxPack_Buffer,_cnt);
    return _cnt;
}

// double targetPoint[5][2]={{-0.12,2.4},{-1.28,3.52},{-3.55,2.2},{-3.55,1}};

typedef unsigned int useconds_t;
inline int usleep(useconds_t us) {
    struct timespec req;
    req.tv_sec  = (long) (us / 1000000U);
    req.tv_nsec = (long)((us % 1000000U)*1000U);
    int status = nanosleep(&req,0);
    return status ? -1 : 0;
}

void getMapData(int8_t *data,int &dataSize , const hectorslam::GridMap& gridMap,MapLockerInterface* mapMutex)
{
    static int lastGetMapUpdateIndex=-100;
    if (lastGetMapUpdateIndex != gridMap.getUpdateIndex())
    {

        int sizeX = gridMap.getSizeX();
        int sizeY = gridMap.getSizeY();

        int size = sizeX * sizeY;
        dataSize=size;
        memset(&data[0], -1, sizeof(int8_t) * size);

        if (mapMutex)
        {
        mapMutex->lockMap();
        }

        for(int i=0; i < size; ++i)
        {
            if(gridMap.isFree(i))
            {
                data[i] = 0;
            }
            else if (gridMap.isOccupied(i))
            {
                data[i] = 100;
            }
        }

        lastGetMapUpdateIndex = gridMap.getUpdateIndex();

        if (mapMutex)
        {
        mapMutex->unlockMap();
        }
    }
}


int main(int argc, char** argv)
{
  threadNum[0]=pthread_create(&lidarThreadHandle,NULL,readLidarThread,NULL);
  threadNum[1]=pthread_create(&socketThreadHandle,NULL,socketSendThread,(void*)TARGETIP);
  threadNum[2]=pthread_create(&searchThreadHandle,NULL,pathShearchThread,NULL);
  threadNum[3]=pthread_create(&shareMemThreadHandle,NULL,getShareMem,NULL);

  sleep(1);

  mySerial=(CSerialConnection*)getSerialPort();
  
  int sysStatus=0;
  int printTimeSplit=0;
  int controlTimeSplit=0;
  int socketSendTimeSplit=0;
  int detectTimeSplit=0;
  float controlTime=0;
  HiRobot::LaserScan lidarScan;
  HectorMappingRos slam;
  int8_t occupMap[512*512]={0};
  int occupMapSize=0;
  Eigen::MatrixXd followPath;
  Eigen::Vector2d localPose;
  Eigen::Vector2d target;

  while(1)
  {
      if(lidarDataIsReady())
      {
          lidarScan=lidarGetData();
          slam.scanCallback(lidarScan);
      }
      if(printTimeSplit==50)//500ms
      {
            printTimeSplit=0;
            HiRobot::RobotPose pose=slam.getPose();
            RobotInfo.pose.X=pose.position.x;
            RobotInfo.pose.Y=pose.position.y;
            RobotInfo.pose.W=pose.orientation.w;
            char tmp[1000];
            sprintf(tmp,"x:%f\ny:%f\nw:%f\n\0",pose.position.x,pose.position.y,pose.orientation.w);
            printf("%s",tmp);
            getMapData(occupMap,occupMapSize,slam.getMap(),slam.getMapLock());
      }
      if(detectTimeSplit==100)//1s
      {
          detectTimeSplit=0;
          if(getBoxValue(personBox))
          {
              float direction=(float)(personBox.LLCX+personBox.ULCX)/2/1920-0.5;
              HiRobot::LaserScan lidarData=lidarGetData();
              float dis=0;
              for(int i=0;i<10;i++)
              {
                  dis+=lidarData.ranges[85+i+floor(direction*10)];
              }
              dis=dis/10-2.5;
              target=localPose;
              target(1)+=dis;
              startSearch(localPose,target);
              sysStatus=1;
          }
      }
      if(socketSendTimeSplit==400)
      {
            RobotInfo.pose.id=0x5555AAAA;
            socketSendData(RobotInfo.data,sizeof(RobotInfo.data));
      }
      if(socketSendTimeSplit==500)//5000ms
      {
            socketSendTimeSplit=0;
            socketSendData(occupMap,occupMapSize);
      }
      if(controlTimeSplit==5) //50ms
      {
          if(sysStatus=1)
          {
              followPath=getOptimizPath();
              if(followPath.rows()!=1){
                  controlTime=0;
                  sysStatus=2;
              } 
          }
          HiRobot::RobotPose pose=slam.getPose();
          localPose(0)=pose.position.x;
          localPose(1)=pose.position.y;
          Eigen::Vector2d carTarget=getPosPoly(followPath,floor(controlTime),controlTime-floor(controlTime));
          Tx_APIC(carTarget);
          if((target-localPose).norm()>0.5)
          {
              carStopCmd();
              startSearch(localPose,target);
              sysStatus=1;
          }
          controlTimeSplit=0;
      }
      usleep(10000);//10ms 时间片
      printTimeSplit++;
      socketSendTimeSplit++;
  }

  return(0);
}

